#include <stdio.h>
#include <math.h>

#define PI 3.14159265358979323846

// 定义关节角度结构体
typedef struct {
    double theta1;  // 基座旋转角度（度）
    double theta2;  // 肩部俯仰角度（度）
    double theta3;  // 肘部俯仰角度（度）
    double theta4;  // 腕部俯仰角度（度）
    double theta5;  // 腕部旋转角度（度）
} JointAngles;

// 定义连杆长度结构体
typedef struct {
    double l1;  // 连杆1长度
    double l2;  // 连杆2长度
    double l3;  // 连杆3长度
    double l4;  // 连杆4长度
    double l5;  // 连杆5长度
} ArmDimensions;

// 定义末端执行器位置结构体
typedef struct {
    double x;
    double y;
    double z;
} EndEffectorPosition;

// 正运动学计算函数
void forwardKinematics(const JointAngles *angles, const ArmDimensions *dim, EndEffectorPosition *position) {
    // 将角度转换为弧度
    double theta1 = angles->theta1 * PI / 180.0;
    double theta2 = angles->theta2 * PI / 180.0;
    double theta3 = angles->theta3 * PI / 180.0;
    double theta4 = angles->theta4 * PI / 180.0;
    double theta5 = angles->theta5 * PI / 180.0;

    // 计算总角度
    double theta234 = theta2 + theta3 + theta4;

    // 计算位置
    position->x = -cos(theta1) * (-dim->l1 + dim->l2 * sin(theta2) + dim->l3 * sin(theta2 + theta3) + (dim->l4 + dim->l5) * sin(theta234));
    position->y = -sin(theta1) * (-dim->l1 + dim->l2 * sin(theta2) + dim->l3 * sin(theta2 + theta3) + (dim->l4 + dim->l5) * sin(theta234));
    position->z = dim->l2 * cos(theta2) + dim->l3 * cos(theta2 + theta3) + (dim->l4 + dim->l5) * cos(theta234);
}

// 更新关节角度并重新计算位置
void updateJointAngles(JointAngles *angles, const ArmDimensions *dim, EndEffectorPosition *position) {
    forwardKinematics(angles, dim, position);
}

int main() {
    // 初始化关节角度
    JointAngles angles = {0.0, 0, 0,0,0};
    
    // 初始化连杆长度
    ArmDimensions dim = {
        .l1 = 0.0f,  
        .l2 = 80.0f,
        .l3 = 100.0f,
        .l4 = 50.0f,
        .l5 = 90.0f
    };

    // 计算末端位置
    EndEffectorPosition position;
    forwardKinematics(&angles, &dim, &position);

    // 修改关节角度
    angles.theta1 = 135;  // 修改肩部俯仰角度
    angles.theta2 = 60;
    angles.theta3 = -90;
    angles.theta4 = -60;
    angles.theta5 = 0;

    updateJointAngles(&angles, &dim, &position);

    // 输出更新后的位置
    printf("Updated Position: x = %f, y = %f, z = %f\n", position.x, position.y, position.z);

    return 0;
}